
#include "FindRectTarget.h"
#include "MiscFunctions.h"

#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdexcept>
#include <stdio.h>

using namespace std;
using namespace cv;
using namespace idch;

FindRectTarget::FindRectTarget( int minContourLength , int imgWidth , int imgHeight)
    : cornerFinder( 30 , 0.3) , lineFinder(80,3,true) , rectFinder(0.2,150)
{
    this->minContourLength = minContourLength;
    this->tempImg = cv::Mat::zeros(imgWidth,imgHeight,CV_8UC1);
    this->imgWidth = imgWidth;
    this->imgHeight = imgHeight;
}

FindRectTarget::~FindRectTarget()
{

}

bool FindRectTarget::process( cv::Mat &binaryImg )
{
    if( imgWidth != binaryImg.cols || imgHeight != binaryImg.rows )
        throw std::runtime_error("Unexpected image shape.");

    corners.clear();

    // remove noise and fill smooth out the contour
    cv::erode(binaryImg,tempImg,Mat());
    cv::dilate(tempImg,binaryImg,Mat());

    // find the contour
    contours0.clear();
    hierarchy.clear();

    findContours( binaryImg, contours0, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);

    contours.clear();

    // discard small contours
    for( size_t i = 0; i < contours0.size(); ++i ) {
        vector<cv::Point> &v = contours0.at(i);

        if( v.size() >= minContourLength ) {
            contours.push_back(v);
        }
    }

    // compute corner features and find lines in the shape
    lines.clear();
    for( size_t i = 0; i < contours.size(); ++i ) {
        vector<cv::Point> &v = contours.at(i);

        cornerFinder.find(v);

        corners.push_back(cornerFinder.getCorners());

        // find lines
        lineFinder.extract(v,cornerFinder.getCorners());

        const std::vector<LineSegmentF> &lines = lineFinder.getFound();

        for( size_t i = 0; i < lines.size(); ++i ) {
            this->lines.push_back( lines.at(i));
        }
    }

    // find rectangular targets
    rectFinder.process(lines);

    // select the best rectangle and convert it into points representation
    return selectBestTarget();
}

const RectPt& FindRectTarget::getTarget() {
    return targetDesc;
}

void FindRectTarget::setConfig( double lineTol ) {
    lineFinder.configure( lineTol );
}

void FindRectTarget::drawMorphological( cv::Mat &img )
{

}

void FindRectTarget::drawContours( cv::Mat &imgRGB )
{
    Scalar color(1,255,255);
    cv::drawContours(imgRGB, contours , -1, color , 3);// CV_FILLED = 1
}

void FindRectTarget::drawCorners( cv::Mat &imgRGB )
{
    Scalar color(255,0,0);
    Scalar outside(0,0,255);

    for( size_t i = 0; i < corners.size(); ++i ) {
        std::vector<ContourCorner> &c = corners.at(i);

        for( size_t j = 0; j < c.size(); j++ ) {
            ContourCorner &o = c.at(j);
            Point p(o.x,o.y);

            cv::circle(imgRGB,p,10,outside,-1);
            cv::circle(imgRGB,p,6,color,-1);
        }
    }
}

void FindRectTarget::drawLines( cv::Mat &img ) {

    Scalar color(255,100,255);

    for( size_t i = 0; i < lines.size(); ++i ) {
        const LineSegmentF &f = lines.at(i);

        drawLineSegment(f,color,img);
    }
}

void FindRectTarget::drawRectLineSeg( cv::Mat &img ) {

    const std::vector<LineSegRect>& rects = rectFinder.getRectangles();

    Scalar color(255,100,255);

    for( size_t i = 0; i < rects.size(); ++i ) {
        const LineSegRect &r = rects.at(i);

        drawLineSegment(r.l,color,img);
        drawLineSegment(r.r,color,img);
        drawLineSegment(r.t,color,img);
        drawLineSegment(r.b,color,img);
    }
}

void FindRectTarget::drawLineSegment( const LineSegmentF &f , const cv::Scalar& color , cv::Mat &img ) {
    cv::Point a((int)f.x0,(int)f.y0);
    cv::Point b((int)f.x1,(int)f.y1);

    cv::line(img,a,b,color,4);
}

bool FindRectTarget::selectBestTarget() {
//    printf("ENTER\n");
    int best = -1;
    double bestScore = 0;

    const std::vector<LineSegRect>& rects = rectFinder.getRectangles();

    for( size_t i = 0; i < rects.size(); i++ ) {
        const LineSegRect &lsr = rects.at(i);

        RectPt r = convertRect(lsr);

        float distR = 0;
        float distLSR = 0;

        distR += idch::distance<float>(r.tl.x,r.tl.y,r.tr.x,r.tr.y);
        distR += idch::distance<float>(r.tl.x,r.tl.y,r.bl.x,r.bl.y);
        distR += idch::distance<float>(r.bl.x,r.bl.y,r.br.x,r.br.y);
        distR += idch::distance<float>(r.tr.x,r.tr.y,r.br.x,r.br.y);

        distLSR += idch::distance<float>(lsr.t.x0,lsr.t.y0,lsr.t.x1,lsr.t.y1);
        distLSR += idch::distance<float>(lsr.b.x0,lsr.b.y0,lsr.b.x1,lsr.b.y1);
        distLSR += idch::distance<float>(lsr.l.x0,lsr.l.y0,lsr.l.x1,lsr.l.y1);
        distLSR += idch::distance<float>(lsr.r.x0,lsr.r.y0,lsr.r.x1,lsr.r.y1);

//        printf(" format ratio %f\n",distLSR/distR);

//        printf(" distR = %f dirstLSR = %f\n",distR,distLSR);

        if( distLSR/distR  > bestScore ) {
            bestScore = distLSR/distR;
            best = i;
        }
    }

    if( best != -1 ) {
        targetDesc = convertRect( rects.at(best));
        return true;
    }
    return false;
}
